
#include <cmath>
#include <cstdlib>
#include <limits>

#include <QPointF>
#include <QtCore>
#include <QtOpenGL>

#include "ai/agent.h"
#include "ai/flock.h"
#include "ai/map.h"
#include "core/engine.h"

Flock::Flock(const Map& rkMap)
    : m_rkMap(rkMap),
      m_pkRanges(new Quadtree< Sphere >()),
      m_eRepulsionFunction(Constant),
      m_repulsionFactor(0.5),
      m_eAlignementFunction(Constant),
      m_alignementFactor(0.5),
      m_eAttractionFunction(Constant),
      m_attractionFactor(0.5),
      m_eContainmentFunction(Exponential),
      m_containmentFactor(5),
      m_containmentRange(1),
      m_kDisturbance(0, 0, 0, 1.5),
      m_eDisturbanceType(None),
      m_disturbanceFactor(0)
{
    Q_ASSERT(m_pkRanges);

    // Initialize quadtree's boundaries.
    const qreal margin = 0.10;

    setBounds(rkMap.object().minX() - rkMap.object().width() * margin,
              rkMap.object().minZ() - rkMap.object().depth() * margin,
              rkMap.object().maxX() + rkMap.object().width() * margin,
              rkMap.object().maxZ() + rkMap.object().depth() * margin);

    clear();
}

Flock::~Flock()
{
    // Delete quadtrees.
    delete m_pkRanges;
    m_pkRanges = NULL;
}

quint32 Flock::count() const
{
    return m_kAgents.count();
}

QSet< Agent* > Flock::agents() const
{
    return m_kAgents;
}

void Flock::add(Agent& rkAgent)
{
    if(m_kAgents.contains(&rkAgent))
        return;

    m_kAgents.insert(&rkAgent);

    m_pkRanges->add(rkAgent.range());
}

void Flock::clear()
{
    m_pkRanges->clear();

    m_kAgents.clear();
}

void Flock::setBounds(const QRectF& rkBounds)
{
    m_pkRanges->setBounds(rkBounds);
}

void Flock::setBounds(qreal x, qreal y, qreal width, qreal height)
{
    m_pkRanges->setBounds(x, y, width, height);
}

bool Flock::remove(Agent& rkAgent)
{
    if(!m_kAgents.remove(&rkAgent))
        return false;

    m_pkRanges->remove(rkAgent.range());

    return true;
}

Quadtree< Sphere >& Flock::ranges() const
{
    return *m_pkRanges;
}

qreal Flock::repulsionFactor() const
{
    return m_repulsionFactor;
}

qreal Flock::alignementFactor() const
{
    return m_alignementFactor;
}

qreal Flock::attractionFactor() const
{
    return m_attractionFactor;
}

qreal Flock::containmentFactor() const
{
    return m_containmentFactor;
}

qreal Flock::containmentRange() const
{
    return m_containmentRange;
}

void Flock::setRepulsionFactor(qreal factor)
{
    m_repulsionFactor = factor;
}

void Flock::setAlignementFactor(qreal factor)
{
    m_alignementFactor = factor;
}

void Flock::setAttractionFactor(qreal factor)
{
    m_attractionFactor = factor;
}

void Flock::setContainmentFactor(qreal factor)
{
    m_containmentFactor = factor;
}

void Flock::setRepulsionFunction(BlendingFunction eType)
{
    m_eRepulsionFunction = eType;
}

void Flock::setAlignementFunction(BlendingFunction eType)
{
    m_eAlignementFunction = eType;
}

void Flock::setAttractionFunction(BlendingFunction eType)
{
    m_eAttractionFunction = eType;
}

void Flock::setContainmentFunction(BlendingFunction eType)
{
    m_eContainmentFunction = eType;
}

void Flock::setContainmentRange(qreal range)
{
    m_containmentRange = range;
}

void Flock::setDisturbanceFunction(BlendingFunction eType)
{
    m_eDisturbanceFunction = eType;
}

void Flock::setDisturbanceType(Disturbance eType)
{
    m_eDisturbanceType = eType;
}

void Flock::setDisturbanceFactor(qreal factor)
{
    m_disturbanceFactor = factor;
}

void Flock::setDisturbancePosition(const QVector3D& rkPosition)
{
    m_kDisturbance.setPosition(rkPosition);
}

void Flock::setDisturbancePosition(qreal x, qreal y, qreal z)
{
    setDisturbancePosition(QVector3D(x, y, z));
}

void Flock::setDisturbanceRange(qreal radius)
{
    m_kDisturbance.setRadius(radius);
}

void Flock::update()
{
    accumulate();
    blend();
    contain();
    disrupt();
}

void Flock::paint() const
{
    glPushAttrib(GL_CURRENT_BIT);

    const qreal flockOffset = 0.2;

    // Quadtree.
    QSet< const Quadtree< Sphere >* > kLeaves = m_pkRanges->leaves();
    foreach(const Quadtree< Sphere >* pkLeaf, kLeaves)
    {
        glBegin(GL_LINE_LOOP);
        glColor3f(0.2, 0.8, 0.8);

        QPointF kTopLeft = pkLeaf->bounds().topLeft();
        QPointF kbottomLeft = pkLeaf->bounds().bottomLeft();
        QPointF kbottomRight = pkLeaf->bounds().bottomRight();
        QPointF ktopRight = pkLeaf->bounds().topRight();

        glVertex3f(kTopLeft.x(), flockOffset, kTopLeft.y());
        glVertex3f(kbottomLeft.x(), flockOffset, kbottomLeft.y());
        glVertex3f(kbottomRight.x(), flockOffset, kbottomRight.y());
        glVertex3f(ktopRight.x(), flockOffset, ktopRight.y());

        glEnd();
    }

    // Ranges.
    qreal step = M_PI * 2 / 20;
    foreach(const Agent* pkAgent, m_kAgents)
    {
        // Only paint ranges of active agents.
        if(!pkAgent->isActive())
            continue;

        qreal sum = 0;

        glBegin(GL_LINE_LOOP);
        glColor3f(0.2, 0.8, 0.8);

        while(sum < M_PI * 2)
        {
            qreal x = qCos(sum) * pkAgent->range().radius() + pkAgent->range().x();
            qreal z = qSin(sum) * pkAgent->range().radius() + pkAgent->range().z();

            glVertex3f(x, flockOffset, z);

            sum += step;
        }

        glEnd();
    }

    glPopAttrib();
}

qreal Flock::factor(BlendingFunction eType, qreal proximityRatio)
{
    switch(eType)
    {
    case Constant:
        return 1;

    case Linear:
        return proximityRatio;

    case Quadratic:
        return qPow(proximityRatio, 2);

    case Exponential:
        return qPow(M_E, proximityRatio);
    }

    // Error if no function type is found.
    Q_ASSERT(false);
    return 0;
}

void Flock::accumulate()
{
    QSet< const Quadtree< Sphere >* > kLeaves = m_pkRanges->leaves();

    foreach(const Quadtree< Sphere >* pkLeaf, kLeaves)
    {
        const QSet< const Sphere* >& rkRanges = pkLeaf->items();

        for(QSet< const Sphere* >::ConstIterator kRangeA = rkRanges.begin();
            kRangeA != rkRanges.end() && kRangeA + 1 != rkRanges.end();
            ++kRangeA)
        {
            Agent* pkA = static_cast< Agent* >((*kRangeA)->userData());
            if(!pkA || !pkA->isActive())
                continue;

            for(QSet< const Sphere* >::ConstIterator kRangeB = kRangeA + 1;
                kRangeB != rkRanges.end();
                ++kRangeB)
            {
                Agent* pkB = static_cast< Agent* >((*kRangeB)->userData());
                if(!pkB || !pkB->isActive())
                    continue;

                // If any intersection, repulse.
                QVector3D kContact;
                if(Sphere::intersection(pkA->range(), pkB->range(), &kContact))
                {
                    qreal distanceSquared = (pkA->range().center() - pkB->range().center()).lengthSquared();
                    qreal thresholdSquared = qPow(pkA->range().radius() + pkB->range().radius(), 2);
                    qreal ratio = distanceSquared / thresholdSquared;

                    // Agent A.
                    QVector3D kRepulsion = QVector3D(pkA->range().center() - kContact).normalized();
                    QVector3D kAlignement = pkB->velocity();
                    QVector3D kAttraction = QVector3D(kContact - pkA->range().center());

                    accumulator(*pkA).m_kRepulsions << kRepulsion * factor(m_eRepulsionFunction, 1 - ratio);
                    accumulator(*pkA).m_kAlignements << kAlignement * factor(m_eAlignementFunction, ratio);
                    accumulator(*pkA).m_kAttractions << kAttraction * factor(m_eAttractionFunction, 1 - ratio);

                    // Agent B.
                    kRepulsion = QVector3D(pkB->range().center() - kContact).normalized();
                    kAlignement = pkA->velocity();
                    kAttraction = QVector3D(kContact - pkB->range().center());

                    accumulator(*pkB).m_kRepulsions << kRepulsion * factor(m_eRepulsionFunction, 1 - ratio);
                    accumulator(*pkB).m_kAlignements << kAlignement * factor(m_eAlignementFunction, 1 - ratio);
                    accumulator(*pkB).m_kAttractions << kAttraction * factor(m_eAttractionFunction, 1 - ratio);
                }
            }
        }
    }
}

void Flock::blend()
{
    for(QHash< Agent*, Accumulator >::ConstIterator kIt = m_kAccumulators.begin();
        kIt != m_kAccumulators.end(); ++kIt)
    {
        Agent* pkAgent = kIt.key();
        Q_ASSERT(pkAgent);

        const Accumulator& rkAccumulator = *kIt;
        QVector3D kContribution;

        // Repulsion.
        QVector3D kRepulsion;
        foreach(const QVector3D& rkContribution, rkAccumulator.m_kRepulsions)
            kRepulsion += rkContribution;

        // Alignement.
        QVector3D kAlignement;
        foreach(const QVector3D& rkContribution, rkAccumulator.m_kAlignements)
            kAlignement += rkContribution;

        if(!kAlignement.isNull())
            kAlignement /= rkAccumulator.m_kAlignements.count();

        // Attraction.
        QVector3D kAttraction;
        foreach(const QVector3D& rkContribution, rkAccumulator.m_kAttractions)
            kAttraction += rkContribution;

        if(!kAttraction.isNull())
            kAttraction /= rkAccumulator.m_kAttractions.count();

        // Average contributions.
        kContribution += kRepulsion * m_repulsionFactor;
        kContribution += kAlignement * m_alignementFactor;
        kContribution += kAttraction * m_attractionFactor;
        kContribution /= 3;

        // Set velocity.
        pkAgent->setVelocity(pkAgent->velocity() + kContribution * Engine::s_updateDelay);
    }

    // Clear the contributions.
    m_kAccumulators.clear();
}

void Flock::contain()
{
    foreach(Agent* pkAgent, m_kAgents)
    {
        // Get candidate walls.
        const QVector3D kP1(pkAgent->position() -
                            QVector3D(-m_containmentRange,
                                      0,
                                      m_containmentRange));
        const QVector3D kP2(pkAgent->position() -
                            QVector3D(m_containmentRange,
                                      0,
                                      -m_containmentRange));
        const Edge3D kDiagonal(kP1, kP2);

        const Quadtree< Edge3D >* pkQuadrant = m_rkMap.walls().findItem(kDiagonal);
        //Q_ASSERT(pkQuadrant);
        if(!pkQuadrant)
            continue;

        const QSet< const Edge3D* >& rkWalls = pkQuadrant->items();

        // Check for every wall.
        qreal closest = std::numeric_limits< qreal >::max();
        QVector3D kAxis;

        foreach(const Edge3D* pkWall, rkWalls)
        {
            // Compute distance to wall.
            QVector3D kNormalized = pkWall->toVector3D().normalized();
            qreal u = QVector3D::dotProduct(kNormalized, pkAgent->position() - pkWall->p1());

            u = qMax(u, 0.);
            u = qMin(u, pkWall->toVector3D().length());

            QVector3D kProjection = kNormalized * u + pkWall->p1();
            QVector3D kCandidateAxis = pkAgent->position() - kProjection;
            qreal kCandidateDistance = kCandidateAxis.length();

            // Check if candidate is retained.
            if(kCandidateDistance <= m_containmentRange && kCandidateDistance < closest)
            {
                closest = kCandidateDistance;
                kAxis = kCandidateAxis;
            }
        }

        // Add the contribution.
        if(!kAxis.isNull())
        {
            qreal ratio = closest / m_containmentRange;
            QVector3D kContribution = kAxis * m_containmentFactor * factor(m_eContainmentFunction,  1 - ratio);

            // Set velocity.
            pkAgent->setVelocity(pkAgent->velocity() + kContribution * Engine::s_updateDelay);
        }
    }
}

void Flock::disrupt()
{
    // Return if disrupt type is none.
    if(m_eDisturbanceType == None)
        return;

    // Return if no quadrant was found.
    const Quadtree< Sphere >* pkQuadrant = m_pkRanges->findItem(m_kDisturbance);
    if(!pkQuadrant)
        return;

    // Find agents.
    const QSet< const Sphere* >& rkRanges = pkQuadrant->items();

    for(QSet< const Sphere* >::ConstIterator kIt = rkRanges.begin();
        kIt != rkRanges.end(); ++kIt)
    {
        Agent* pkAgent = static_cast< Agent* >((*kIt)->userData());
        if(!pkAgent || !pkAgent->isActive())
            continue;

        // If any intersection compute disrupt effect.
        if(Sphere::intersection(pkAgent->range(), m_kDisturbance, NULL))
        {
            // disrupt effect is computed relatively to proximity.
            qreal distanceSquared = (pkAgent->range().center() - m_kDisturbance.center()).lengthSquared();
            qreal thresholdSquared = qPow(pkAgent->range().radius(), 2);
            qreal ratio = distanceSquared / thresholdSquared;

            QVector3D kVelocity = pkAgent->velocity();
            QVector3D kContribution;

            if(m_eDisturbanceType == Repulsive)
                kContribution = QVector3D(pkAgent->range().center() - m_kDisturbance.center()).normalized();
            else
                kContribution = QVector3D(m_kDisturbance.center() - pkAgent->range().center()).normalized();

            kContribution *= m_disturbanceFactor * factor(m_eDisturbanceFunction, 1 - ratio);

            // Set velocity.
            pkAgent->setVelocity(pkAgent->velocity() + kContribution * Engine::s_updateDelay);
        }
    }
}

Flock::Accumulator& Flock::accumulator(Agent& rkAgent)
{
    QHash< Agent*, Accumulator >::Iterator kIt = m_kAccumulators.find(&rkAgent);

    // There is already an entry in the contribution hash.
    if(kIt != m_kAccumulators.end())
        return *kIt;

    // There is no entry in the contribution hash.
    return *(m_kAccumulators.insert(&rkAgent, Accumulator()));
}

